#include "method.hpp"
void METHOD::searchCenter(cv::Mat &mat)
{
    cv::Mat a[3];
    cv::split(mat, a);
    cv::Mat m1 = a[2];
    std::vector<std::vector<cv::Point>> contours;
    std::vector<cv::Vec4i> hierarchy;
    cv::findContours(m1, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_NONE, cv::Point(0, 0)); //CCOMP->EXTERNAL
    std::vector<cv::RotatedRect> ros;
    for (int i = 0; i < contours.size(); i++)
    {
        ros.push_back(cv::minAreaRect(contours[i]));
        if (hierarchy[i][3] != -1 && hierarchy[i][0] == -1)
        {
            cv::Point2f pts[4];
            ros[i].points(pts);
            if (ros[i].size.area() < 2000 && ros[i].size.area() > 1000)
            {
                rec_add = ros[i].center;
            }
        }
        else if (hierarchy[i][3] == -1 && hierarchy[i][0] != -1)
        {
            cv::Point2f pts[4];
            ros[i].points(pts);
            if (ros[i].size.area() < 1000)
            {
                center_add = ros[i].center;
            }
        }
    }

    cv::circle(mat, center_add, 5, cv::Scalar(0, 255, 0), cv::FILLED);
    cv::circle(mat, rec_add, 5, cv::Scalar(0, 255, 0), cv::FILLED);
}
void METHOD::leadFirstVector(double time)
{
    begin[0] = -center_add.x + rec_add.x;
    begin[1] = -center_add.y + rec_add.y;
    vector_before = begin;
    t0 = time;
    time_before = t0;
    angle = 0;
}

double METHOD::calculate(double time) //calculate the angle btwn before and now
{
    double tmp1 = 1.305 * (time - t0);
    double tmp2 = f * (cos(1.884 * time + angle) - cos(1.884 * t0 + angle));
    return tmp1 - tmp2;
}
double METHOD::Huber(double delta, double time_now)
{
    if (delta >= -3.5 && delta <= 3.5)
        return delta * (0.785 * cos(1.884 * time_now + angle));
    else if (delta > 3.5)
        return (0.785 * cos(1.884 * time_now + angle)) * 3.5;
    else
        return -(0.785 * cos(1.884 * time_now + angle)) * 3.5;
}
void METHOD::calculateRadius(double time_now, cv::Mat &mat1)
{
    Eigen::Vector2d vector_now(-center_add.x + rec_add.x, -center_add.y + rec_add.y);
    Eigen::Vector2d vect_vertical(-vector_now[1], vector_now[0]);
    double angle_delta = vector_before.dot(vector_now) / (vector_now.norm() * vector_before.norm());
    angle_delta = acos(angle_delta);
    double v_now = angle_delta / (time_now - time_before);
    if (vect_vertical.dot(vector_before) < 0)
        v_now = -v_now;
    double v_predict = 0.785 * sin(1.884 * time_now + angle) + 1.305;
    double tmp = v_predict - v_now;
    double angle_btwn = -1 * calculate(time_now);
    Eigen::Matrix2d mat;
    mat << cos(angle_btwn), -sin(angle_btwn),
        sin(angle_btwn), cos(angle_btwn);
    Eigen::Vector2d vector_predict = mat * begin;
    if((vector_predict-vector_now).norm()>9)
    {
        double huber = Huber(tmp, time_now);
        double foot = 0.1;
        angle = angle - foot * huber;
    }
    cv::Point2f point_predict(center_add.x + vector_predict[0], center_add.y + vector_predict[1]);
    cv::circle(mat1, point_predict, 5, cv::Scalar(255, 0, 0), cv::FILLED);
    vector_before = vector_now;
    time_before = time_now;
    if(angle<0)angle=2*M_PI+angle;
    else if(angle>2*M_PI)angle=angle-2*M_PI;
}
